using BepuUtilities;
using System.Diagnostics;

namespace BepuPhysics
{
    /// <summary>
    /// 按以下顺序更新模拟：睡眠->预测身体边界框->碰撞检测->循环{接触数据更新(如果迭代>0)->整合身体速度->解算器->整合身体姿势}->数据结构优化。
    /// 每个内部循环执行都模拟长度为dt/substepCount的子时间步长。
    /// 适用于具有难以求解的约束系统的模拟,这些约束系统需要较短的时间步长,但不需要高频碰撞检测。
    /// </summary>
    public class SubsteppingTimestepper : ITimestepper
    {
        /// <summary>
        /// 获取或设置在每个TimeStep期间要执行的子步数。
        /// </summary>
        public int SubstepCount { get; set; }

        /// <summary>
        /// 在休眠程序完成之后、身体集成之前激发。
        /// </summary>
        public event TimestepperStageHandler Slept;
        /// <summary>
        /// 在实体针对帧的预测运动更新其边界框之后、碰撞检测之前激发。
        /// </summary>
        public event TimestepperStageHandler BeforeCollisionDetection;
        /// <summary>
        /// 在识别所有冲突之后、但在子步骤循环开始之前激发。
        /// </summary>
        public event TimestepperStageHandler CollisionsDetected;
        /// <summary>
        /// 在子步骤开始时激发。
        /// </summary>
        public event TimestepperSubstepStageHandler SubstepStarted;
        /// <summary>
        /// 在第一个和第一个速度积分之前的子步开始处增量更新接触约束之后的触发。
        /// </summary>
        public event TimestepperSubstepStageHandler ContactConstraintsUpdatedForSubstep;
        /// <summary>
        /// 在实体的速度积分之后、解算器执行之前激发。
        /// </summary>
        public event TimestepperSubstepStageHandler VelocitiesIntegrated;
        /// <summary>
        /// 在解算器执行之后、身体姿势集成之前激发。
        /// </summary>
        public event TimestepperSubstepStageHandler ConstraintsSolved;
        /// <summary>
        /// 在实体集成其姿势之后和子步骤结束之前激发。
        /// </summary>
        public event TimestepperSubstepStageHandler PosesIntegrated;
        /// <summary>
        /// 在子步骤结束时激发。
        /// </summary>
        public event TimestepperSubstepStageHandler SubstepEnded;
        /// <summary>
        /// 在所有子步骤执行完毕、数据结构进行增量优化之前激发。
        /// </summary>
        public event TimestepperStageHandler SubstepsComplete;

        public SubsteppingTimestepper(int substepCount)
        {
            SubstepCount = substepCount;
        }

        public void Timestep(Simulation simulation, float dt, IThreadDispatcher threadDispatcher = null)
        {
            simulation.Sleep(threadDispatcher);
            Slept?.Invoke(dt, threadDispatcher);

            simulation.PredictBoundingBoxes(dt, threadDispatcher);
            BeforeCollisionDetection?.Invoke(dt, threadDispatcher);

            simulation.CollisionDetection(dt, threadDispatcher);
            CollisionsDetected?.Invoke(dt, threadDispatcher);

            Debug.Assert(SubstepCount >= 0, "Substep count should be positive.");
            var substepDt = dt / SubstepCount;

            for (int substepIndex = 0; substepIndex < SubstepCount; ++substepIndex)
            {
                SubstepStarted?.Invoke(substepIndex, dt, threadDispatcher);
                if (substepIndex > 0)
                {
                    // 这取代了子步骤的碰撞检测。它使用当前速度来更新穿透深度。
                    // 这绝对是近似值,但对于避免一些明显奇怪的行为很重要。
                    // 请注意,我们不会在第一次迭代中运行此操作-上面的实际冲突检测会处理它。
                    simulation.IncrementallyUpdateContactConstraints(substepDt, threadDispatcher);
                    ContactConstraintsUpdatedForSubstep?.Invoke(substepIndex, dt, threadDispatcher);
                }
                simulation.IntegrateVelocitiesAndUpdateInertias(substepDt, threadDispatcher);
                VelocitiesIntegrated?.Invoke(substepIndex, dt, threadDispatcher);

                simulation.Solve(substepDt, threadDispatcher);
                ConstraintsSolved?.Invoke(substepIndex, dt, threadDispatcher);

                simulation.IntegratePoses(substepDt, threadDispatcher);
                PosesIntegrated?.Invoke(substepIndex, dt, threadDispatcher);
                SubstepEnded?.Invoke(substepIndex, dt, threadDispatcher);
            }
            SubstepsComplete?.Invoke(dt, threadDispatcher);

            simulation.IncrementallyOptimizeDataStructures(threadDispatcher);
        }
    }
}
